/*
  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
*/

#define FAST_SQRT

using System;
using System.Collections.Generic;
using System.Text;

using Microsoft.Xna.Framework;

namespace XnaDevRu.BulletX
{
	public static class MathHelper
	{
		internal const float Sqrt12 = 0.7071067811865475244008443621048490f;
		internal const float Infinity = 3.402823466e+38f;
		internal const float Epsilon = 1.192092896e-07f;

		// 0x5f3759df = original, 0x5f375a86 = from paper
		internal const int MagicConstant = 0x5f3759df;

		public static float Sqrt( float number )
		{
#if !FAST_SQRT
			return ( float )Math.Sqrt( number );
#else
			unsafe
			{
				float half = number * 0.5f;
				int i = *( int* )&number;
				i = MagicConstant - ( i >> 1 );
				float y = *( float* )&i;
				y = y * ( 1.5f - ( half * y * y ) );
				y = y * ( 1.5f - ( half * y * y ) ); // 2nd iteration - better result
				return number * y;
			}
#endif
		}

		public static float InvSqrt( float number )
		{
#if !FAST_SQRT
			return 1.0f / ( float )Math.Sqrt( number );
#else
			unsafe
			{
				float half = 0.5f * number;
				int i = *( int* )&number;
				i = MagicConstant - ( i >> 1 );
				float y = *( float* )&i;
				y = y * ( 1.5f - half * y * y );
				y = y * ( 1.5f - half * y * y ); // 2nd iteration - better result
				return y;
			}
#endif
		}

		public static Vector3 MatrixToVector( ref Matrix m, ref Vector3 v )
		{
			Vector3 vr1 = new Vector3( m.M11, m.M12, m.M13 );
			Vector3 vr2 = new Vector3( m.M21, m.M22, m.M23 );
			Vector3 vr3 = new Vector3( m.M31, m.M32, m.M33 );
			float x, y, z;
			Vector3.Dot( ref vr1, ref v, out x );
			Vector3.Dot( ref vr2, ref v, out y );
			Vector3.Dot( ref vr3, ref v, out z );
			return new Vector3( x + m.Translation.X, y + m.Translation.Y, z + m.Translation.Z );
		}

		public static Vector3 MatrixToVector( Matrix m, ref Vector3 v )
		{
			Vector3 vr1 = new Vector3( m.M11, m.M12, m.M13 );
			Vector3 vr2 = new Vector3( m.M21, m.M22, m.M23 );
			Vector3 vr3 = new Vector3( m.M31, m.M32, m.M33 );
			float x, y, z;
			Vector3.Dot( ref vr1, ref v, out x );
			Vector3.Dot( ref vr2, ref v, out y );
			Vector3.Dot( ref vr3, ref v, out z );
			return new Vector3( x + m.Translation.X, y + m.Translation.Y, z + m.Translation.Z );
		}

		internal static int ClosestAxis( ref Vector4 v )
		{
			// Absolute( v )
			Vector4 abs = new Vector4( Math.Abs( v.X ), Math.Abs( v.Y ), Math.Abs( v.Z ), Math.Abs( v.W ) );
			return MaxAxis( ref abs );
		}

		internal static Vector4 Absolute( ref Vector4 v )
		{
			return new Vector4( Math.Abs( v.X ), Math.Abs( v.Y ), Math.Abs( v.Z ), Math.Abs( v.W ) );
		}

		internal static int MaxAxis( ref Vector4 v )
		{
			int maxIndex = -1;
			float maxVal = float.MinValue;
			if( v.X > maxVal )
			{
				maxIndex = 0;
				maxVal = v.X;
			}
			if( v.Y > maxVal )
			{
				maxIndex = 1;
				maxVal = v.Y;
			}
			if( v.Z > maxVal )
			{
				maxIndex = 2;
				maxVal = v.Z;
			}
			if( v.W > maxVal )
			{
				maxIndex = 3;
				maxVal = v.W;
			}

			return maxIndex;
		}

		internal static int MaxAxis( ref Vector3 v )
		{
			return v.X < v.Y ? ( v.Y < v.Z ? 2 : 1 ) : ( v.X < v.Z ? 2 : 0 );
		}

		// conservative test for overlap between two aabbs
		internal static bool TestAabbAgainstAabb2( ref Vector3 aabbMinA, ref Vector3 aabbMaxA, ref Vector3 aabbMinB, ref Vector3 aabbMaxB )
		{
			bool overlap = true;
			overlap = ( aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X ) ? false : overlap;
			overlap = ( aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z ) ? false : overlap;
			overlap = ( aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y ) ? false : overlap;
			return overlap;
		}

		internal static bool TestTriangleAgainstAabb2( Vector3[] vertices, ref Vector3 aabbMin, ref Vector3 aabbMax )
		{
			Vector3 p1 = vertices[ 0 ];
			Vector3 p2 = vertices[ 1 ];
			Vector3 p3 = vertices[ 2 ];

			if( Math.Min( Math.Min( p1.X, p2.X ), p3.X ) > aabbMax.X )
				return false;
			if( Math.Max( Math.Max( p1.X, p2.X ), p3.X ) < aabbMin.X )
				return false;

			if( Math.Min( Math.Min( p1.Z, p2.Z ), p3.Z ) > aabbMax.Z )
				return false;
			if( Math.Max( Math.Max( p1.Z, p2.Z ), p3.Z ) < aabbMin.Z )
				return false;

			if( Math.Min( Math.Min( p1.Y, p2.Y ), p3.Y ) > aabbMax.Y )
				return false;
			if( Math.Max( Math.Max( p1.Y, p2.Y ), p3.Y ) < aabbMin.Y )
				return false;
			return true;
		}

		internal static void SetInterpolate3( ref Vector3 vA, ref Vector3 vB, float rt, ref Vector3 interpolated )
		{
			float s = 1.0f - rt;
			interpolated.X = s * vA.X + rt * vB.X;
			interpolated.Y = s * vA.Y + rt * vB.Y;
			interpolated.Z = s * vA.Z + rt * vB.Z;
		}

		internal static void PlaneSpace1( Vector3 n, ref Vector3 p, ref Vector3 q )
		{
			if( Math.Abs( n.Z ) > Sqrt12 )
			{
				// choose p in y-z plane
				float a = n.Y * n.Y + n.Z * n.Z;
				float k = 1f / ( float )Math.Sqrt( a );
				p.X = 0;
				p.Y = -n.Z * k;
				p.Z = n.Y * k;
				// set q = n x p
				q.X = a * k;
				q.Y = -n.X * p.Z;
				q.Z = n.X * p.Y;
			}
			else
			{
				// choose p in x-y plane
				float a = n.X * n.X + n.Y * n.Y;
				float k = 1f / ( float )Math.Sqrt( a );
				p.X = -n.Y * k;
				p.Y = n.X * k;
				p.Z = 0;
				// set q = n x p
				q.X = -n.Z * p.Y;
				q.Y = n.Z * p.X;
				q.Z = a * k;
			}
		}

		internal static bool RayAabb( ref Vector3 rayFrom,
								 ref Vector3 rayTo,
								 ref Vector3 aabbMin,
								 ref Vector3 aabbMax,
								 float param, ref Vector3 normal )
		{
			Vector3 aabbHalfExtent = ( aabbMax - aabbMin ) * 0.5f;
			Vector3 aabbCenter = ( aabbMax + aabbMin ) * 0.5f;
			Vector3 source = rayFrom - aabbCenter;
			Vector3 target = rayTo - aabbCenter;
			int sourceOutcode = Outcode( source, aabbHalfExtent );
			int targetOutcode = Outcode( target, aabbHalfExtent );
			if( ( sourceOutcode & targetOutcode ) == 0x0 )
			{
				float lambda_enter = 0;
				float lambda_exit = param;
				Vector3 r = target - source;
				float normSign = 1;
				Vector3 hitNormal = new Vector3();
				int bit = 1;

				for( int j = 0; j < 2; j++ )
				{
					{
						if( ( sourceOutcode & bit ) != 0 )
						{
							float lambda = ( -source.X - aabbHalfExtent.X * normSign ) / r.X;
							if( lambda_enter <= lambda )
							{
								lambda_enter = lambda;
								hitNormal = new Vector3();
								hitNormal.X = normSign;
							}
						}
						else if( ( targetOutcode & bit ) != 0 )
						{
							float lambda = ( -source.X - aabbHalfExtent.X * normSign ) / r.X;
							SetMin( ref lambda_exit, lambda );
						}
						bit <<= 1;
					}
					{
						if( ( sourceOutcode & bit ) != 0 )
						{
							float lambda = ( -source.Y - aabbHalfExtent.Y * normSign ) / r.Y;
							if( lambda_enter <= lambda )
							{
								lambda_enter = lambda;
								hitNormal = new Vector3();
								hitNormal.Y = normSign;
							}
						}
						else if( ( targetOutcode & bit ) != 0 )
						{
							float lambda = ( -source.Y - aabbHalfExtent.Y * normSign ) / r.Y;
							SetMin( ref lambda_exit, lambda );
						}
						bit <<= 1;
					}
					{
						if( ( sourceOutcode & bit ) != 0 )
						{
							float lambda = ( -source.Z - aabbHalfExtent.Z * normSign ) / r.Z;
							if( lambda_enter <= lambda )
							{
								lambda_enter = lambda;
								hitNormal = new Vector3();
								hitNormal.Z = normSign;
							}
						}
						else if( ( targetOutcode & bit ) != 0 )
						{
							float lambda = ( -source.Z - aabbHalfExtent.Z * normSign ) / r.Z;
							SetMin( ref lambda_exit, lambda );
						}
						bit <<= 1;
					}
					normSign = -1;
				}
				if( lambda_enter <= lambda_exit )
				{
					param = lambda_enter;
					normal = hitNormal;
					return true;
				}
			}
			return false;
		}

		internal static void SetMin( ref float a, float b )
		{
			if( a > b )
				a = b;
		}

		internal static void SetMax( ref float a, float b )
		{
			if( a < b )
				a = b;
		}

		internal static void SetMax( ref Vector3 self, ref Vector3 other )
		{
			if( other.X > self.X )
				self.X = other.X;

			if( other.Y > self.Y )
				self.Y = other.Y;

			if( other.Z > self.Z )
				self.Z = other.Z;
		}

		internal static Vector3 SetMax( Vector3 self, Vector3 other )
		{
			if( other.X > self.X )
				self.X = other.X;

			if( other.Y > self.Y )
				self.Y = other.Y;

			if( other.Z > self.Z )
				self.Z = other.Z;

			return self;
		}

		internal static void SetMin( ref Vector3 self, ref Vector3 other )
		{
			if( other.X < self.X )
				self.X = other.X;

			if( other.Y < self.Y )
				self.Y = other.Y;

			if( other.Z < self.Z )
				self.Z = other.Z;
		}

		internal static Vector3 SetMin( Vector3 self, Vector3 other )
		{
			if( other.X < self.X )
				self.X = other.X;

			if( other.Y < self.Y )
				self.Y = other.Y;

			if( other.Z < self.Z )
				self.Z = other.Z;

			return self;
		}

		internal static int Outcode( Vector3 p, Vector3 halfExtent )
		{
			return ( p.X < -halfExtent.X ? 0x01 : 0x0 ) |
				   ( p.X > halfExtent.X ? 0x08 : 0x0 ) |
				   ( p.Y < -halfExtent.Y ? 0x02 : 0x0 ) |
				   ( p.Y > halfExtent.Y ? 0x10 : 0x0 ) |
				   ( p.Z < -halfExtent.Z ? 0x4 : 0x0 ) |
				   ( p.Z > halfExtent.Z ? 0x20 : 0x0 );
		}

		internal static Matrix Absolute( Matrix m )
		{
			return new Matrix( Math.Abs( m.M11 ), Math.Abs( m.M12 ), Math.Abs( m.M13 ), Math.Abs( m.M14 ),
							  Math.Abs( m.M21 ), Math.Abs( m.M22 ), Math.Abs( m.M23 ), Math.Abs( m.M24 ),
							  Math.Abs( m.M31 ), Math.Abs( m.M32 ), Math.Abs( m.M33 ), Math.Abs( m.M34 ),
							  Math.Abs( m.M41 ), Math.Abs( m.M42 ), Math.Abs( m.M43 ), Math.Abs( m.M44 ) );
		}

		internal static void Absolute( ref Matrix m, out Matrix v )
		{
			v = new Matrix( Math.Abs( m.M11 ), Math.Abs( m.M12 ), Math.Abs( m.M13 ), Math.Abs( m.M14 ),
							  Math.Abs( m.M21 ), Math.Abs( m.M22 ), Math.Abs( m.M23 ), Math.Abs( m.M24 ),
							  Math.Abs( m.M31 ), Math.Abs( m.M32 ), Math.Abs( m.M33 ), Math.Abs( m.M34 ),
							  Math.Abs( m.M41 ), Math.Abs( m.M42 ), Math.Abs( m.M43 ), Math.Abs( m.M44 ) );
		}

		internal static void SetValueByIndex( ref Vector3 v, int i, float value )
		{
			if( i == 0 )
				v.X = value;
			else if( i == 1 )
				v.Y = value;
			else
				v.Z = value;
		}

		internal static float GetValueByIndex( Vector3 v, int i )
		{
			if( i == 0 )
				return v.X;
			else if( i == 1 )
				return v.Y;
			else
				return v.Z;
		}

		internal static Vector3 InvXForm( Matrix m, Vector3 v )
		{
			v -= m.Translation;
			m.Translation = new Vector3();
			return MathHelper.Transform( v, Matrix.Transpose( m ) );
		}

		internal static Matrix InverseTimes( Matrix m, Matrix t )
		{
			Vector3 v = t.Translation - m.Translation;

			Matrix mat = TransposeTimes( m, t );
			mat.Translation = Vector3.Transform( v, m );
			return mat;
		}

		internal static Matrix TransposeTimes( Matrix mA, Matrix mB )
		{
			return new Matrix(
				mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31,
				mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32,
				mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,
				0,
				mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31,
				mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32,
				mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,
				0,
				mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31,
				mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32,
				mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,
				0, 0, 0, 0, 1 );
		}

		internal static Vector3 GetColumn( Matrix m, int column )
		{
			switch( column )
			{
				case 1:
					return new Vector3( m.M11, m.M21, m.M31 );
				case 2:
					return new Vector3( m.M12, m.M22, m.M32 );
				case 3:
					return new Vector3( m.M13, m.M23, m.M33 );
				default:
					throw new ArgumentOutOfRangeException( "column" );
			}
		}

		internal static Vector3 GetRow( Matrix m, int row )
		{
			switch( row )
			{
				case 1:
					return new Vector3( m.M11, m.M12, m.M13 );
				case 2:
					return new Vector3( m.M21, m.M22, m.M23 );
				case 3:
					return new Vector3( m.M31, m.M32, m.M33 );
				default:
					throw new ArgumentOutOfRangeException( "row" );
			}
		}

		internal static Quaternion GetRotation( Matrix m )
		{
			float trace = m.M11 + m.M22 + m.M33;
			Quaternion q = new Quaternion();

			if( trace > 0 )
			{
				float s = ( float )Math.Sqrt( trace + 1.0f );
				q.W = s * 0.5f;
				s = 0.5f / s;

				q.X = ( m.M32 - m.M23 ) * s;
				q.Y = ( m.M13 - m.M31 ) * s;
				q.Z = ( m.M21 - m.M12 ) * s;
			}
			else
			{
				int i = m.M11 < m.M22 ?
						( m.M22 < m.M33 ? 2 : 1 ) :
						( m.M11 < m.M33 ? 2 : 0 );
				int j = ( i + 1 ) % 3;
				int k = ( i + 2 ) % 3;

				float s = ( float )Math.Sqrt( GetElement( m, i, i ) - GetElement( m, j, j ) - GetElement( m, k, k ) + 1.0f );
				SetElement( ref q, i, s * 0.5f );
				s = 0.5f / s;

				q.W = ( GetElement( m, k, j ) - GetElement( m, j, k ) ) * s;
				SetElement( ref q, j, ( GetElement( m, j, i ) + GetElement( m, i, j ) ) * s );
				SetElement( ref q, k, ( GetElement( m, k, i ) + GetElement( m, i, k ) ) * s );
			}
			return q;
		}

		internal static float SetElement( ref Quaternion q, int index, float value )
		{
			switch( index )
			{
				case 0:
					q.X = value;
					break;
				case 1:
					q.Y = value;
					break;
				case 2:
					q.Z = value;
					break;
				case 3:
					q.W = value;
					break;
			}

			return 0;
		}

		internal static float GetElement( Quaternion q, int index )
		{
			switch( index )
			{
				case 0:
					return q.X;
				case 1:
					return q.Y;
				case 2:
					return q.Z;
				default:
					return q.W;
			}
		}

		internal static float GetElement( Matrix mat, int index )
		{
			int row = index % 3;
			int col = index / 3;

			return GetElement( mat, row, col );
		}

		internal static float GetElement( Matrix mat, int row, int col )
		{
			switch( row )
			{
				case 0:
					switch( col )
					{
						case 0:
							return mat.M11;
						case 1:
							return mat.M12;
						case 2:
							return mat.M13;
					}
					break;
				case 1:
					switch( col )
					{
						case 0:
							return mat.M21;
						case 1:
							return mat.M22;
						case 2:
							return mat.M23;
					}
					break;
				case 2:
					switch( col )
					{
						case 0:
							return mat.M31;
						case 1:
							return mat.M32;
						case 2:
							return mat.M33;
					}
					break;
			}

			return 0;
		}

		internal static float GetElement( Vector3 v, int index )
		{
			if( index == 0 )
				return v.X;
			if( index == 1 )
				return v.Y;
			if( index == 2 )
				return v.Z;

			throw new ArgumentOutOfRangeException( "index" );
		}

		internal static void SetElement( ref Vector3 v, int index, float value )
		{
			if( index == 0 )
				v.X = value;
			else if( index == 1 )
				v.Y = value;
			else if( index == 2 )
				v.Z = value;
			else
				throw new ArgumentOutOfRangeException( "index" );
		}

		public static Matrix InvertMatrix( Matrix m )
		{
			Vector3 pos = m.Translation;
			m.Translation = Vector3.Zero;
			Matrix inv = Matrix.Transpose( m );
			pos = Vector3.Transform( -pos, m );
			inv.Translation = pos;
			return inv;
		}

		public static Matrix GetDisplayMatrix( Matrix m )
		{
			Matrix displayMatrix = m;
			displayMatrix.Translation = Vector3.Zero;
			displayMatrix = Matrix.Transpose( displayMatrix );
			displayMatrix.Translation = m.Translation;
			return displayMatrix;
		}

		internal static Vector3 Transform( Vector3 position, Matrix matrix )
		{
			Vector3 vector = new Vector3();
			vector.X = ( ( ( position.X * matrix.M11 ) + ( position.Y * matrix.M12 ) ) + ( position.Z * matrix.M13 ) ) + matrix.M41;
			vector.Y = ( ( ( position.X * matrix.M21 ) + ( position.Y * matrix.M22 ) ) + ( position.Z * matrix.M23 ) ) + matrix.M42;
			vector.Z = ( ( ( position.X * matrix.M31 ) + ( position.Y * matrix.M32 ) ) + ( position.Z * matrix.M33 ) ) + matrix.M43;
			return vector;
		}

		internal static Vector3 TransformNormal( Vector3 position, Matrix matrix )
		{
			Vector3 vector = new Vector3();
			vector.X = ( ( ( position.X * matrix.M11 ) + ( position.Y * matrix.M12 ) ) + ( position.Z * matrix.M13 ) );
			vector.Y = ( ( ( position.X * matrix.M21 ) + ( position.Y * matrix.M22 ) ) + ( position.Z * matrix.M23 ) );
			vector.Z = ( ( ( position.X * matrix.M31 ) + ( position.Y * matrix.M32 ) ) + ( position.Z * matrix.M33 ) );
			return vector;
		}
	}
}
